//------------------------------------------------------------------------------
// Fichiers d'ent�te utilis�s
//------------------------------------------------------------------------------
#include "HoughTransformationOperator.hpp"
#include "Tools/mat44.hpp"
#include "Tools/Logger.hpp"
#include <iostream>
#include <math.h>


//------------------------------------------------------------------------------
// Définition de constantes
//------------------------------------------------------------------------------
#define ACCUMULATION_THRESHOLD 10

namespace ImageAnalysis
{
	//----------------------------------------------------------------------
	// Constructeur
	//----------------------------------------------------------------------
	HoughTransformationOperator::HoughTransformationOperator( Gradiant* gradiant, BinaryImage* refinedImage, SegmentContainer::TSegmentContainerList* segmentContainerList ):
	currentGradiant( gradiant ),
	currentRefinedImage( refinedImage ),
	currentSegmentContainerList( segmentContainerList )
	{

	}

	//----------------------------------------------------------------------
	// Destructeur
	//----------------------------------------------------------------------
	HoughTransformationOperator::~HoughTransformationOperator( )
	{

	}

	//----------------------------------------------------------------------
	//
	//----------------------------------------------------------------------
	void HoughTransformationOperator::Process( )
	{
		int imageWidth  = currentGradiant->GetWidth();
		int imageHeight = currentGradiant->GetHeight();		

		// Pas de discrisation
		int maximumRho   = (int)sqrt(imageHeight*imageHeight + imageWidth*imageWidth);
		int maximumTheta = 270;
		int imageSize    = maximumRho*maximumTheta;
		//float factorTheta = imageWidth / 270.0f;		
		//float factorRho   = imageHeight / maxLenght;
		//Tools::ILogger::Log("Discretisation [rho:%f, theta:%f]\n", factorRho, factorTheta );

		int* parameterSpace = new int[maximumTheta*maximumRho];
		for( int s=0; s<imageSize; ++s )
		{	parameterSpace[s] = 0; }

		for( int y=0; y<imageHeight; ++y )
		{
			for( int x=0; x<imageWidth; ++x )
			{
				int pixelIndex = x + y*imageWidth;
				unsigned char value;
				value = *currentRefinedImage->GetPixel( pixelIndex );
				if( value == BinaryImage::WHITE_VALUE )
				{
					// Recuperation de l'orientation du gradiant
					float theta = currentGradiant->GetGradiantOrientation( pixelIndex );

					// Calcul de la distance
					float angle = theta*M_PI/180.0f;
					float rho = x*cos( angle ) + y*sin( angle );

					// Calcul des valeurs approximee
					//int approximateTheta = (int)(theta * factorTheta);
					//int approximateRho   = (int)(rho * factorRho);
					Tools::ILogger::Log("Pixel[x:%d,y:%d] Value[rho:%4.2f,theta:%f4.2]\n", x, y, rho, theta );
					
					parameterSpace[(int)(theta) + ((int)rho)*maximumTheta] += 1;
					//parameterSpace[(int)(approximateTheta) + ((int)approximateRho)*imageWidth] += 1;
				}
			}
		}

		SegmentContainer* currentSegmentContainer = new SegmentContainer( imageWidth, imageHeight );
		for( int r=0; r<maximumRho; ++r )
		{
			for( int t=0; t<maximumTheta; ++t )
			{
				int parameterIndex = t + r*maximumTheta;
				int value = parameterSpace[ parameterIndex ];
				if( value > ACCUMULATION_THRESHOLD )
				{

					float xAxis[3];
					Tools::vec3_init( xAxis, 1, 0, 0 );
					float zAxis[3];
					Tools::vec3_init( zAxis, 0, 0, 1 );						
					float normal[3];
					Tools::vec3_init( normal, 1, 0, 0 );

					//float angle = (t*270.0f/(float)imageWidth)*(M_PI/180.0f); // angle en radians
					//float angle = (t*270.0f/(float)imageWidth); // angle en degres
					//float rho = r/factorRho;
					//Tools::ILogger::Log("Used Value[rho:%d,theta:%d]=%d\n", r, angle );

					float rotationMatrix[16];						
					Tools::mat44_identity(rotationMatrix);
					Tools::mat44_rotate(rotationMatrix,t,0,0,1);
					Tools::mat44_mul_vec( normal, rotationMatrix, xAxis);
					Tools::vec3_norm(normal,normal);

					float direction[3] ;
					Tools::vec3_cross(direction, normal, zAxis );

					// Positionnement du point
					Tools::vec3_const_mul(normal, r, normal);

					float point1[3];
					float point2[3];
					Tools::vec3_copy(point1, direction);
					Tools::vec3_copy(point2, direction);
					Tools::vec3_const_mul(point1,  maximumRho, point1);
					Tools::vec3_const_mul(point2, -maximumRho, point2);
					Tools::vec3_add( point1, normal, point1 );
					Tools::vec3_add( point2, normal, point2 );

					Tools::ILogger::Log("Retained Line[rho:%d,theta:%d]=%d\n", r, t, value );
					currentSegmentContainer->AddSegment( new SegmentContainer::Segment( (int)point1[0], (int)point1[1], (int)point2[0], (int)point2[1] ) );					
				}
			}
 		}
		currentSegmentContainerList->push_back( currentSegmentContainer );
	}
}


